﻿#include "visualcommon.h"
#include <QFile>
#include <QTextCodec>
#include <cmath>
#include <QCoreApplication>

#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/extract_clusters.h>
#include <opencv2/imgproc.hpp>

#include "commonfunctionset.h"
#include "eigenfuncset.h"
#include "pclfun.h"
#include "common_log.h"

#define CENG_ERROR (50) // 层高误差

VisualCommon::VisualCommon()
{}

VisualCommon::~VisualCommon()
{}

int VisualCommon::Init(const visAlgParam& param)
{
	if (param.calibration.isEmpty()) {
		log_error("calibration is empty");
		return -99;
	}
	QString path = QCoreApplication::applicationDirPath() + "/config/" + param.calibration;
	if (!InitCalibration(path)) {
		log_error("标定文件【{}】初始化失败，检查标定文件是否存在，", param.calibration);
		return -1;
	}
	m_dir = param.dir;
	m_maxNum = param.maxNum;
	if (m_dir < 0 || m_dir > 4) {
		log_error("拆垛方向配置异常");
		return -1;
	}

	log_trace("Init successed");
	return 0;
}

bool VisualCommon::Caculate(const VisAlgInput& input, Position& pos, VisAlgOutput& out)
{
	log_trace("Caculate start: rgb:{}, depth:{}, rec:{}, sku:{},{},{}, num:{}", input.rgb,
		input.depth, input.boxs.size(), input.sku.length, input.sku.width, input.sku.height,
		input.taskNum);
	if (!CheckParam(input, out.msg)) {
		out.num = -1;
		log_error("input param error");
		return false;
	}
	// 如果存在上次结果，进行高度检查
	/********暂时未实现**********/
	if (input.boxs.size() == 0) {
		out.msg = "托盘未识别到货物";
		out.num = 0;
		return true;
	}
	//读取彩色图
	cv::Mat rgb = cv::imread(input.rgb.toStdString());
	// 创建深度图点云
	PointCloud::Ptr cloud_dep = GetDepthPointCloud(input.depth);
	if (m_save) {
		SavePCDFile(cloud_dep, "cloud_dep");
	}
	
	// 获取箱子深度图点云
	PointCloud::Ptr cloudin(new PointCloud);	// 使用新点云   改变Z的单位
	for (int i = 0; i < cloud_dep->size(); i++) {
		PointT point_tempa;
		point_tempa.x = cloud_dep->at(i).x;
		point_tempa.y = cloud_dep->at(i).y;
		point_tempa.z = cloud_dep->at(i).z / 1000.0;
		cloudin->push_back(point_tempa);
	}
	int i = 0;
	QVector<PointCloud::Ptr> cloud_boxs;
	for (auto& iter : input.boxs) {
		auto temp = GetBoxCloud(cloudin, iter);
		cloud_boxs.append(temp);
		
		cv::Point p1(iter.x1, iter.y1);
		cv::Point p2(iter.x2, iter.y2);
		cv::Point p3(iter.x3, iter.y3);
		cv::Point p4(iter.x4, iter.y4);
		std::vector<cv::Point> points = { p1,p2,p3,p4 };
		DrawRcog(rgb, points);
		i++;
	}
	log_trace("Get box num = {}", cloud_boxs.size());	// 识别的箱子数量
	// 将深度图点云转换到机械手坐标系
	PointCloud::Ptr cloud_dep_w1 = TranToWorld(cloud_dep);


	//// 切割深度图点云，保留范围内点云
	PointCloud::Ptr cloud_dep_w = CutCloudToPlate(cloud_dep_w1);
	SavePCDFile(cloud_dep_w, "cloud_dep_w");
	// 点云单位转换
	for (int i = 0; i < cloud_dep_w->size(); i++) {
		cloud_dep_w->at(i).x = cloud_dep_w->at(i).x / 1000.0;
		cloud_dep_w->at(i).y /= 1000.0;
		cloud_dep_w->at(i).z /= 1000.0;
	}
	if (m_save) {
		SavePCDFile(cloud_dep_w, "cloud_dep1000");
	}
	// 点云聚类
	std::vector<PointCloud::Ptr> vCloud;
	EuclideanClusterExtractionCloud(cloud_dep_w, 0.02, 50, 9999999, &vCloud);
	// 计算聚类点云中的最大高度
	double maxHeight = MathMaxCloud(vCloud) * 1000;
	log_trace("max Height {}", maxHeight);
	cv::Mat depthori = cv::imread(input.depth.toStdString(), -1);
	int h = depthori.rows;
	int w = depthori.cols;


	DrowPlate(rgb, maxHeight);
	// 箱子点云处理, 获取箱子的处理点云和顶点坐标(图片坐标系)
	QVector<visual::BoxInfo> boxs;
	i = 0;
	for (auto& iter : cloud_boxs) {
		if (iter->points.size() < 300) {
			continue;
		}
		//SavePCDFile(iter, "BoxCloud" + QString::number(i));
		PointCloud::Ptr box_cam = TransToCamera(iter);	//转换到相机坐标系
		//SavePCDFile(box_cam, "BoxCloud_cam" + QString::number(i));
		box_cam = PclFun::CropCloudNormalRange(box_cam, 30, 'z');
		std::vector<PointCloud::Ptr> clouds;
		EuclideanClusterExtractionCloud(box_cam, 0.03, 50, 9999, &clouds);	// 聚类， 3cm
		if (clouds.size() <= 0) {	// 过滤无法聚类的箱子
			continue;
		}
		//获取最大点云
		PointCloud::Ptr Cloud3 = PclFun::GetMaxCloud(clouds);

		// 平面分割
		pcl::ModelCoefficients::Ptr pcoefficients(new pcl::ModelCoefficients);
		PointCloud::Ptr pCloudPlane = PclFun::PlaneSegmentation(Cloud3, 0.02, pcoefficients); //平面分割参数设置为1cm
		auto result = PclFun::PlaneCoefficients2Euler(pcoefficients);
		float za = result.at(2);
		float anglex = 30;
		// 验证点云方向
		if (!(abs(za) < anglex || abs(za - 180) < anglex || abs(za + 180) < anglex)) {
			continue;
		}
		// 从相机坐标系转回像素坐标系
		auto cloud_p = TransToPix(pCloudPlane);
		SavePCDFile(cloud_p, "BoxCloud_back" + QString::number(i));

		// 获取旋转矩形区域
		cv::RotatedRect rect1 = MathBoxCvRect(pCloudPlane, h, w);
		DrowBox(rgb, rect1);
		// 计算箱子的信息
		visual::BoxInfo info = GetBoxInfo(pCloudPlane, rect1);
		if (info.length == 0 || info.width == 0) {
			continue;
		}
		info.id = i;
		boxs.push_back(info);
		i++;
	}

	if (boxs.size() == 0) {
		out.msg = "物料未在托盘范围内";
		out.num = -1;
		out.rgb = ImgEncodeToByte(rgb, ".png");
		return false;
	}

	// 需要找最上层的箱子
	std::sort(boxs.begin(), boxs.end(), [](const visual::BoxInfo& a, const visual::BoxInfo& b) {
		return a.pos.z > b.pos.z;
		});
	int z = boxs.at(0).pos.z;
	QVector<visual::BoxInfo> box_z;
	for (auto& iter : boxs) {
		if (z - iter.pos.z <= CENG_ERROR) {
			box_z.append(iter);
		} else {
			break;
		}
	}
	
	// 对同层的箱子进行排序，找到优先抓取的箱子
	visual::BoxInfo  target = {};
	if (input.taskNum >= m_maxNum) {
		target = FindGrabBoxGroup(box_z, m_maxNum);
	} else {
		target = FindGrabBoxGroup(box_z, input.taskNum);
	}
	log_trace("taget: id{}, ({}, {}), ({}, {}, {}, {})", i, target.width, target.length,
		target.pos.x, target.pos.y, target.pos.z, target.pos.a);

	out.msg = "ok";
	out.rgb = ImgEncodeToByte(rgb, ".png");
	// 创建夹具点云
	PointCloud::Ptr cloud_jig = CreateJigCloud(1); // 1:纯吸盘，2：侧吸

	return true;
}

// 查找抓取箱子组
typedef QVector<visual::BoxInfo> BoxGroup;
visual::BoxInfo VisualCommon::FindGrabBoxGroup(QVector<visual::BoxInfo>& pos, int grabNum)
{
	SortSkuPos(pos);
	if (grabNum == 1) {
		return pos.at(0);
	}
	QVector<BoxGroup> group;
	// 根据箱子的旋转角度对箱子进行分组
	QVector<BoxGroup> dirgorup;
	BoxGroup temp;
	for (auto& iter : pos) {
		if (temp.isEmpty()) {
			temp.push_back(iter);
		} else {
			if (std::abs(iter.pos.a - temp[0].pos.a) < 10) {	// 箱子之间的夹角小于10度，便认为方向一致
				temp.push_back(iter);
			} else {
				dirgorup.push_back(temp);
				temp.clear();
			}
		}
	}
	if (temp.size() > 0) {
		dirgorup.push_back(temp);
	}
	
	// 根据X和Y的值对箱子进行分组		
	for (auto& gru : dirgorup) {
		temp.clear();
		for (auto& iter : gru) {
			if (temp.isEmpty() ) {
				temp.push_back(iter);
			} else {
				// x值接近 y值之间相差不超过宽度，说明箱子沿着宽度方向并排
				if ((std::abs(iter.pos.x - temp[0].pos.x) < 30 && 
						std::abs(iter.pos.y - temp[0].pos.y) < std::max(iter.width, temp[0].width) + 30) || 
					(std::abs(iter.pos.y - temp[0].pos.y) < 30 && 
						std::abs(iter.pos.x - temp[0].pos.x) < std::max(iter.width, temp[0].width) + 30)) {
					temp.push_back(iter);
				} else {
					group.push_back(temp);
					temp.clear();
				}
				if (temp.size() == grabNum) {
					group.push_back(temp);
					temp.clear();
				}
			}
		}
		if (temp.size() > 0) {
			group.push_back(temp);
		}
	}

	// 将组内箱子进行融合
	QVector<visual::BoxInfo> fusion;
	int i = 0;
	for (auto& iter : group) {
		visual::BoxInfo info = {};
		for (auto box : iter) {
			info.width += box.width;
			info.length += box.length;
			info.pos.x += box.pos.x;
			info.pos.y += box.pos.y;
			info.pos.z += box.pos.z;
			info.pos.a += box.pos.a;
		}
		int num = iter.size();
		info.width /= num;
		info.length /= num;
		info.pos.x /= num;
		info.pos.y /= num;
		info.pos.z /= num;
		info.pos.a /= num;
		info.id = i;
		log_trace("id:{}, ({}, {}), ({}, {}, {}, {})", i, info.width, info.length,
			info.pos.x, info.pos.y, info.pos.z, info.pos.a);
		fusion.push_back(info);
		i++;
	}
	SortSkuPos(fusion);
	return fusion.at(0);
}


// 计算箱子尺寸信息和位置信息
visual::BoxInfo VisualCommon::GetBoxInfo(PointCloud::Ptr cloud, cv::RotatedRect& rect)
{
	visual::BoxInfo info = {};
	// 点云单位转换
	for (int i = 0; i < cloud->points.size(); i++) {
		cloud->points[i].z *= 1000;	 // m->mm;
	}
	// 点云平面分割处理
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	// Create the segmentation object
	pcl::SACSegmentation<PointT> seg;
	PointCloud::Ptr cloud_plane(new PointCloud);
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(500);
	seg.setDistanceThreshold(5);//单位mm
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);
	// 提取索引
	pcl::ExtractIndices<PointT> extract;
	// Extract the inliers
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);       // 设置分割后的内点为需要提取的点集
	extract.setNegative(false);        // 设置提取内点
	extract.filter(*cloud_plane);          // 提取并保存至 cloud_p

	// 计算平均高度
	double height = 0;
	for (int i = 0; i < cloud_plane->points.size(); i++) {
		height += cloud_plane->points[i].z;
	}
	height = height / cloud_plane->points.size();

	// 获取箱子顶点
	cv::Point2f box[4];
	rect.points(box);
	// 构建箱子点云(图片坐标系)	// 由四个顶点创建
	PointCloud::Ptr box_cloud(new PointCloud);
	box_cloud->push_back(PointT(box[0].x, box[0].y, height));
	box_cloud->push_back(PointT(box[1].x, box[1].y, height));
	box_cloud->push_back(PointT(box[2].x, box[2].y, height));
	box_cloud->push_back(PointT(box[3].x, box[3].y, height));

	auto box_w = TranToWorld(box_cloud);	// 转换到世界坐标系
	box_w->height = 1;
	box_w->width = box_cloud->points.size();

	// 过滤托盘范围之外的点云
	pcl::CropBox<PointT> plate_filter;
	plate_filter.setMax(Eigen::Vector4f(m_plateMax.x, m_plateMax.y, m_plateMax.z, 1.0f));
	plate_filter.setMin(Eigen::Vector4f(m_plateMin.x, m_plateMin.y, m_plateMin.z, 1.0f));
	plate_filter.setInputCloud(box_w);
	plate_filter.setNegative(false);
	PointCloud::Ptr box_w1(new PointCloud);
	plate_filter.filter(*box_w1);

	// 箱子点云如果被裁剪，点的数量小于4，说明箱子在托盘外
	if (box_w1->points.size() < 4) {
		return {};
	}
	//判断识别区域点云值是否为nan
	for (size_t i = 0; i < box_w1->points.size(); i++) {
		if (isnan(box_w1->points[i].x)) {
			return {};
		}
	}
	// 求箱子的长宽高
	int dis1, dis2, dis3;
	using std::sqrt, std::pow;
	dis1 = int(sqrt(pow(box_w1->at(0).x - box_w1->at(1).x, 2) + pow(box_w1->at(0).y - box_w1->at(1).y, 2)));
	dis2 = int(sqrt(pow(box_w1->at(0).x - box_w1->at(2).x, 2) + pow(box_w1->at(0).y - box_w1->at(2).y, 2)));
	dis3 = int(sqrt(pow(box_w1->at(0).x - box_w1->at(3).x, 2) + pow(box_w1->at(0).y - box_w1->at(3).y, 2)));

	info.width = std::min(std::min(dis1, dis2), dis3);
	info.length = dis1 + dis2 + dis3 - std::max(std::max(dis1, dis2), dis3) - info.width;
	log_trace("math1 （{} {}）", info.length, info.width);

	Eigen::Matrix3f rotationalMatrix;
	Eigen::Vector3f size;
	pcl::PointXYZ pos = PclFun::CloudOBB(box_w1, rotationalMatrix, size);
	log_trace("OBB({}, {}, {})", size.x(), size.y(), size.z());
	info.cloud = box_w1;
	info.pos.x = pos.x;
	info.pos.y = pos.y;
	info.pos.z = pos.z;
	info.pos.b = 0;
	info.pos.c = 180;

	log_trace("OBB pos: ({}, {},{})", pos.x, pos.y, pos.z);
	//角度范围
//考虑 cv:: fastatanfast   abs  保留第一象限角度值
	if (dis1 == info.length) {
		if (box_w1->at(0).x == box_w1->at(1).x) {
			info.pos.a = 0;
		} else {
			info.pos.a = 180 * std::atan((box_w1->at(0).y - box_w1->at(1).y) / (box_w1->at(0).x - box_w1->at(1).x)) / M_PI;
		}
	} else if (dis2 == info.length) {	// 永远不能成立
		if (box_w1->at(0).x == box_w1->at(2).x) {
			info.pos.a = 90;
		} else {
			info.pos.a = 180 * std::atan((box_w1->at(0).y - box_w1->at(2).y) / (box_w1->at(0).x - box_w1->at(2).x)) / M_PI;
		}
	} else {
		if (box_w1->at(0).x == box_w1->at(3).x) {
			info.pos.a = 90;
		} else {
			info.pos.a = 180 * std::atan((box_w1->at(0).y - box_w1->at(3).y) / (box_w1->at(0).x - box_w1->at(3).x)) / M_PI;
		}
	}
	if (info.pos.a < 0) {
		info.pos.a = -info.pos.a;
	}

	return info;
}

//下采样的mask上采样
void VisualCommon::UpsampleMask(cv::Mat mask, cv::Mat& upMask, int pad)
{
	upMask = mask.clone();
	for (int i = 0; i < mask.rows; i++) {
		for (int j = 0; j < mask.cols; j++) {
			if (mask.at<uchar>(i, j) != 0) {
				for (int p = -pad; p <= pad; p++) {
					for (int q = -pad; q <= pad; q++) {
						if (i + p < 0 || j + q < 0 || i + p >= mask.rows || j + q >= mask.cols) {
							continue;
						} else {
							upMask.at<uchar>(i + p, j + q) = 255;
						}
					}
				}
			}
		}
	}
	return;
}

// 获取最小区域轮廓
cv::RotatedRect VisualCommon::GetMinAreaRect(cv::Mat mask)
{
	if (mask.empty()) {
		return {};
	}
	cv::Mat canny_output = mask;
	std::vector<std::vector<cv::Point>> contours;//轮廓
	std::vector<cv::Vec4i> hireachy;//层次结构
	cv::findContours(canny_output, contours, hireachy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0));//寻找边缘的轮廓

	//swx bug
	if (contours.size() == 0) {
		return {};
	}

	float maxw = 0;
	float maxh = 0;
	float maxArea = 0;//最大面积
	double degree = 0;
	int t_max = 0;//最大轮廓的序号
	std::vector<cv::RotatedRect> cminRect(contours.size());
	for (size_t t = 0; t < contours.size(); t++) {
		cv::RotatedRect minRect = cv::minAreaRect(contours[t]);//轮廓最小外接矩形
		cminRect[t] = minRect;
		float area = minRect.size.width * minRect.size.height;
		if (area > maxArea && (area < 0.95 * canny_output.size().area()))//小于整张图片的面积的0.99倍 过滤最大的图片边缘
		{//寻找最大面积的外接矩形
			maxArea = area;
			t_max = t;//记录序号
		}
	}

	cv::RotatedRect minRect = cminRect[t_max];//获取轮廓最小外接矩形
	return minRect;
}

// 根据点云转换为cv的选中矩形
cv::RotatedRect VisualCommon::MathBoxCvRect(PointCloud::Ptr cloud, int h, int w)
{
	cv::Mat mask = PclFun::PCLCloud2cvCloudMask(cloud, h, w, m_intriMatrix, m_distorMatrix);
	cv::Mat matMaskPad;
	int pad = 5;
	UpsampleMask(mask, matMaskPad, pad);
	cv::RotatedRect rect = GetMinAreaRect(matMaskPad);
	rect.size.width -= 2 * pad;
	rect.size.height -= 2 * pad;
	return rect;
}

// 计算聚类中的最大高度
double VisualCommon::MathMaxCloud(std::vector<PointCloud::Ptr> vCloudPtr)
{
	double maxHight = -10000;
	for (auto& iter : vCloudPtr) {
		//剪切特定方向点云 //根据向量方向进行裁剪
		iter = PclFun::CropCloudNormalRange(iter, 40, 'z');
		// 平面分开
		pcl::ModelCoefficients::Ptr pcoefficients(new pcl::ModelCoefficients);
		PointCloud::Ptr pCloudPlane = PclFun::PlaneSegmentation(iter, 0.02, pcoefficients); // 分割参数设置为2cm
		if (pCloudPlane->points.size() < 300) {
			continue;
		}

		//计算平面角度
		std::vector<float> result = PclFun::PlaneCoefficients2Euler(pcoefficients);
		//todo  fGamma验证点云方向
		float fGamma = result.at(2);
		float anglex = 30;
		if (!(abs(fGamma) < anglex || abs(fGamma - 180) < anglex || abs(fGamma + 180) < anglex)) {
			continue;
		}

		//计算平均高度
		double h = 0;
		for (int j = 0; j < pCloudPlane->points.size(); j++) {
			h += pCloudPlane->points[j].z;
		}
		h = h / pCloudPlane->points.size();
		if (h > maxHight) {
			maxHight = h;
		}
	}
	return maxHight;
}

//欧式聚类提取 （欧几里得聚类提取）
void VisualCommon::EuclideanClusterExtractionCloud(const PointCloud::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr)
{
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);

	std::vector<pcl::PointIndices> cluster_indices;//提取的聚类
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//欧式聚类提取 （欧几里得聚类提取）
	ec.setClusterTolerance(fClusterTolerance); //簇的容差 2cm
	ec.setMinClusterSize(iMinClusterSize);//最小簇
	ec.setMaxClusterSize(iMaxClusterSize);//最大簇
	ec.setSearchMethod(tree);//kd树查找模式
	ec.setInputCloud(pCloud);
	ec.extract(cluster_indices);//提取

	//遍历每一个聚类
	int j = 0;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
			cloud_cluster->points.push_back(pCloud->points[*pit]);
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		if (cloud_cluster->points.size() < 300)
			continue;
		vCloudPtr.push_back(cloud_cluster);
		j++;
	}
	*pvCloudPtr = vCloudPtr;
}

// 切割点云到托盘范围
PointCloud::Ptr VisualCommon::CutCloudToPlate(PointCloud::Ptr cloud)
{
	PointCloud::Ptr cloud_filteredx(new PointCloud());
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("x");
	pass.setFilterLimits(m_plateMin.x, m_plateMax.x);    //选取0-1之间
	//pass.setFilterLimits(-3, 3);
	pass.filter(*cloud_filteredx);    //过滤

	PointCloud::Ptr cloud_filteredy(new PointCloud());
	pass.setInputCloud(cloud_filteredx);
	pass.setFilterFieldName("y");
	pass.setFilterLimits(m_plateMin.y, m_plateMax.y);    //选取0-1之间
	//pass.setFilterLimits(-1, 3);
	pass.filter(*cloud_filteredy);    //过滤

	PointCloud::Ptr cloud_filteredz(new PointCloud());;
	pass.setInputCloud(cloud_filteredy);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(m_plateMin.z, m_plateMax.z);    //选取0-1之间
	pass.filter(*cloud_filteredz);    //过滤

	return cloud_filteredz;
}

// 点云转换到世界坐标系
PointCloud::Ptr VisualCommon::TranToWorld(PointCloud::Ptr cloud)
{
	PointCloud::Ptr res(new PointCloud);
	Eigen::Matrix4f external;
	res->width = cloud->width;
	res->height = cloud->height;
	res->is_dense = cloud->is_dense;
	PointCloud::Ptr cloud_copy(new PointCloud());
	cloud_copy->points.resize(cloud->points.size());

	// 只有1个标定矩阵时, 内参外参表示为同一个矩阵
	if (m_intri[0][0] == 0) {
		for (int i = 0; i < cloud->size(); i++) {
			cloud_copy->at(i).x = cloud->at(i).x * cloud->at(i).z;
			cloud_copy->at(i).y = cloud->at(i).y * cloud->at(i).z;
			cloud_copy->at(i).z = cloud->at(i).z;
		}
		//相机系-》世界系
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				external(i, j) = m_external[i][j];
			}
		}
	} else {//图像系-》相机系
		for (int i = 0; i < cloud->points.size(); i++) {
			cloud_copy->points.at(i).x = (cloud->points.at(i).x - m_intri[0][2]) * cloud->points.at(i).z / m_intri[0][0];//已经乘过Z了
			cloud_copy->points.at(i).y = (cloud->points.at(i).y - m_intri[1][2]) * cloud->points.at(i).z / m_intri[1][1];
			cloud_copy->at(i).z = cloud->at(i).z;
		}
		//相机系-》世界系
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				external(i, j) = m_external[i][j];
			}
		}
	}
	pcl::transformPointCloud(*cloud_copy, *res, external);
	return res;
}

// 点云转换到相机坐标系
PointCloud::Ptr VisualCommon::TransToCamera(PointCloud::Ptr cloud)
{
	PointCloud::Ptr cloud_copy(new PointCloud());
	cloud_copy->points.resize(cloud->points.size());
	for (int i = 0; i < cloud->points.size(); i++) {
		cloud_copy->points.at(i).x = (cloud->points.at(i).x - m_intri[0][2]) * cloud->points.at(i).z / m_intri[0][0];//已经乘过Z了
		cloud_copy->points.at(i).y = (cloud->points.at(i).y - m_intri[1][2]) * cloud->points.at(i).z / m_intri[1][1];
		cloud_copy->at(i).z = cloud->at(i).z;	
	}
	cloud_copy->width = cloud_copy->points.size();
	cloud_copy->height = 1;
	return cloud_copy;
}

PointCloud::Ptr VisualCommon::TransToPix(PointCloud::Ptr cloud)
{
	PointCloud::Ptr cloud_copy(new PointCloud());
	cloud_copy->points.resize(cloud->points.size());
	for (int i = 0; i < cloud->points.size(); i++) {
		cloud_copy->points.at(i).x = (cloud->points.at(i).x * m_intri[0][0]) / cloud->points.at(i).z + m_intri[0][2];
		cloud_copy->points.at(i).y = (cloud->points.at(i).y * m_intri[1][1]) / cloud->points.at(i).z + m_intri[1][2];
		cloud_copy->at(i).z = cloud->at(i).z;
	}
	cloud_copy->width = cloud_copy->points.size();
	cloud_copy->height = 1;
	return cloud_copy;
}

// 创建深度图点云（相机坐标系）
PointCloud::Ptr VisualCommon::GetDepthPointCloud(const QString& depfile)
{
	PointCloud::Ptr cloud_input_ptr(new PointCloud);
	cv::Mat depthori = cv::imread(depfile.toStdString(), -1);
	int h = depthori.rows;
	int w = depthori.cols;
	cv::Mat gray;
	// 转换格式
	if (depthori.type() == CV_16UC1) {
		depthori.convertTo(depthori, CV_32FC1);
		gray = depthori;
	} else {
		cv::Mat depth_float(depthori.rows, depthori.cols, CV_32FC1, depthori.data);//转32位图像
		gray = depth_float;
	}
	if (gray.empty()) {
		return nullptr;
	}
	for (int x = 0; x < h; x++) {
		for (int y = 0; y < w; y++) {
			PointT point_temp;
			float zz = gray.at<float>(x, y);
			point_temp.z = zz;
			point_temp.x = y;
			point_temp.y = x;
			if (zz <= 5000 && zz > 0) {	// 忽略异常值
				cloud_input_ptr->points.push_back(point_temp);
			}
		}
	}
	cloud_input_ptr->height = 1;
	cloud_input_ptr->width = cloud_input_ptr->points.size();

	return Filter(cloud_input_ptr);
}

// 点云过滤操作
PointCloud::Ptr VisualCommon::Filter(PointCloud::Ptr cloud_in)
{
	//降采样
	PointCloud::Ptr cloud_lowresolution_ptr(new PointCloud);
	pcl::VoxelGrid<PointT> vg_filter;
	vg_filter.setInputCloud(cloud_in);
	vg_filter.setLeafSize(5.0f, 5.0f, 10.0f);
	vg_filter.filter(*cloud_lowresolution_ptr);

	//离群点去除
	PointCloud::Ptr cloud_filter_ptr(new PointCloud);
	pcl::StatisticalOutlierRemoval <pcl::PointXYZ> filt;
	filt.setInputCloud(cloud_lowresolution_ptr);
	filt.setMeanK(1);
	filt.setStddevMulThresh(10.0);
	filt.filter(*cloud_filter_ptr);

	return cloud_filter_ptr;
}

// 切割箱子点云
PointCloud::Ptr VisualCommon::GetBoxCloud(PointCloud::Ptr cloud_in, const BoxPoint& box)
{
	PointCloud::Ptr convex(new PointCloud);
	convex->push_back(PointT(box.x1, box.y1, 0));
	convex->push_back(PointT(box.x2, box.y2, 0));
	convex->push_back(PointT(box.x3, box.y3, 0));
	convex->push_back(PointT(box.x4, box.y4, 0));
	//  计算凸包点
	pcl::ConvexHull<PointT> hull;
	hull.setInputCloud(convex);
	hull.setDimension(2);
	std::vector<pcl::Vertices> polygons;
	PointCloud::Ptr surface_hull(new PointCloud);
	hull.reconstruct(*surface_hull, polygons);
	//CropHull滤波器
	PointCloud::Ptr temp(new PointCloud);
	pcl::CropHull<PointT> ch_filter;
	ch_filter.setInputCloud(cloud_in);
	ch_filter.setDim(2);
	ch_filter.setHullIndices(polygons);
	ch_filter.setHullCloud(surface_hull);
	ch_filter.filter(*temp);

	return temp;
}

#define EXPAND (20)

#define JIG_X (400)
#define JIG_Y (800)

PointCloud::Ptr VisualCommon::CreateJigCloud(int type)
{
	PointCloud::Ptr jig(new PointCloud);
	int jigx = JIG_X / 2;
	int jigy = JIG_Y / 2;

	if (type == 1) {	// 纯吸盘
		jig->push_back(PointT(jigx, jigy, -EXPAND));
		jig->push_back(PointT(-jigx, jigy, -EXPAND));
		jig->push_back(PointT(jigx, -jigy, -EXPAND));
		jig->push_back(PointT(-jigx, -jigy, -EXPAND));
		jig->push_back(PointT(jigx, jigy, EXPAND * 5));
		jig->push_back(PointT(-jigx, jigy, EXPAND * 5));
		jig->push_back(PointT(jigx, -jigy, EXPAND * 5));
		jig->push_back(PointT(-jigx, -jigy, EXPAND * 5));
	} else {	// 带侧吸
		jig->push_back(PointT(jigx, jigy, -EXPAND));
		jig->push_back(PointT(-jigx, jigy, -EXPAND));
		jig->push_back(PointT(jigx, -jigy - 250, -EXPAND));
		jig->push_back(PointT(-jigx, -jigy - 250, -EXPAND));
		jig->push_back(PointT(jigx, jigy, EXPAND * 5));
		jig->push_back(PointT(-jigx, jigy, EXPAND * 5));
		jig->push_back(PointT(jigx, -jigy - 250, EXPAND * 5));
		jig->push_back(PointT(-jigx, -jigy - 250, EXPAND * 5));
	}
	pcl::io::savePCDFile("cloud_jig_original.pcd", *jig);
	return jig;
}


bool VisualCommon::CheckParam(const VisAlgInput& input, QString& msg)
{
	// 参数校验
	if (input.rgb.isEmpty()) {
		msg = "彩色图路径为空";
		return false;
	}

	if (input.depth.isEmpty()) {
		msg = "深度图路径为空";
		return false;
	}

	if (input.taskNum <= 0) {
		msg = "抓取数量为 0";
		return false;
	}

	return true;
}

json_struct(IntrinsicInfo,
	mapping(float, fx);
mapping(float, fy);
mapping(float, cx);
mapping(float, cy);
prop(&fx, &fy, &cx, &cy);
)

json_struct(ExternalInfo,
	mappingt(float, r00, "R_00");
mappingt(float, r01, "R_01");
mappingt(float, r02, "R_02");
mappingt(float, t00, "T_00");
mappingt(float, r10, "R_10");
mappingt(float, r11, "R_11");
mappingt(float, r12, "R_12");
mappingt(float, t01, "T_01");
mappingt(float, r20, "R_20");
mappingt(float, r21, "R_21");
mappingt(float, r22, "R_22");
mappingt(float, t02, "T_02");
prop(&r00,&r01, &r02, &t00, &r10, &r11, &r12, &t01, &r20, &r21, &r22, &t02);
)
json_struct(PlateRangInfo,
	mapping(float, x_min);
mapping(float, x_max);
mapping(float, y_min);
mapping(float, y_max);
mapping(float, z_min);
mapping(float, z_max);
prop(&x_min, &x_max, &y_min, &y_max, &z_min, &z_max);
)

json_struct(CalibrationInfo,
	mapping(IntrinsicInfo, Intrinsic);
mapping(ExternalInfo, External);
mapping(PlateRangInfo, UnstackSpace);
prop(&Intrinsic, &External, &UnstackSpace);
)

bool VisualCommon::InitCalibration(const QString& file)
{

	QFile f(file);
	if (!f.open(QIODevice::ReadOnly | QIODevice::Text)) {
		log_error("标定文件打开失败，{}", file);
		return false;
	}
	QByteArray data = f.readAll();
	f.close();
	CalibrationInfo info = {};
	info.fromJson(data);
	// 内参和畸变参数
	m_intri.clear();
	m_intriMatrix = cv::Mat::eye(3, 3, CV_32FC1);
	m_distorMatrix = cv::Mat::eye(1, 5, CV_32FC1);
	auto& intri = info.Intrinsic();
	m_intri.push_back({ intri.fx(), 0.0f, intri.cx() });
	m_intri.push_back({ 0.0f, intri.fy(),  intri.cy() });
	m_intri.push_back({ 0.0f, 0.0f, 1.0f });

	m_intriMatrix.at<float>(0, 0) = intri.fx();
	m_intriMatrix.at<float>(0, 2) = intri.cx();
	m_intriMatrix.at<float>(1, 1) = intri.fy();
	m_intriMatrix.at<float>(1, 2) = intri.cy();


	// 设置外参
	m_external.clear();
	m_exteMatrix = cv::Mat::eye(4, 4, CV_64FC1);
	auto& exte = info.External();
	m_external.push_back({ exte.r00(), exte.r01(), exte.r02(), exte.t00() });
	m_external.push_back({ exte.r10(), exte.r11(), exte.r12(), exte.t01() });
	m_external.push_back({ exte.r20(), exte.r21(), exte.r22(), exte.t02() });
	m_external.push_back({ 0.0f, 0.0f, 0.0f, 1.0f });
	m_exteMatrix.at<float>(0, 0) = exte.r00();
	//m_exteMatrix.at<float>(0, 1) = exte.r01();
	m_exteMatrix.at<float>(0, 2) = exte.r02();
	m_exteMatrix.at<float>(0, 3) = exte.t00();
	m_exteMatrix.at<float>(1, 0) = exte.r10();
	m_exteMatrix.at<float>(1, 1) = exte.r11();
	m_exteMatrix.at<float>(1, 2) = exte.r12();
	m_exteMatrix.at<float>(1, 3) = exte.t01();
	m_exteMatrix.at<float>(2, 0) = exte.r20();
	m_exteMatrix.at<float>(2, 1) = exte.r21();
	m_exteMatrix.at<float>(2, 2) = exte.r22();
	m_exteMatrix.at<float>(2, 3) = exte.t02();

	// 设置拆垛托盘空间
	auto& plate = info.UnstackSpace();
	m_plateMin.x = plate.x_min();
	m_plateMin.y = plate.y_min();
	m_plateMin.z = plate.z_min();
	m_plateMax.x = plate.x_max();
	m_plateMax.y = plate.y_max();
	m_plateMax.z = plate.z_max();

	return true;
}

void VisualCommon::DrawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Scalar sc, cv::Mat& image)
{
	cv::Mat externalMatrixToCam = m_exteMatrix.inv();
	Eigen::Matrix4f tocam = Eigen::Matrix4f::Identity();
	if (!externalMatrixToCam.empty()) {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				tocam(i, j) = externalMatrixToCam.at<double>(i, j) / 1000;//double转float
			}
		}
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr palltCamCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *palltCamCloud, tocam);
	pcl::PointXYZ left_top_point(palltCamCloud->points.at(0).x, palltCamCloud->points.at(0).y, palltCamCloud->points.at(0).z);
	pcl::PointXYZ left_bottom_point(palltCamCloud->points.at(1).x, palltCamCloud->points.at(1).y, palltCamCloud->points.at(1).z);
	pcl::PointXYZ right_bottom_point(palltCamCloud->points.at(2).x, palltCamCloud->points.at(2).y, palltCamCloud->points.at(2).z);
	pcl::PointXYZ right_top_point(palltCamCloud->points.at(3).x, palltCamCloud->points.at(3).y, palltCamCloud->points.at(3).z);

	//4个角点的顺序不对，需要排序后，再计算底部中心点
	//基于相机方向（向下为Y增加，向右为X增加）且空间中箱子基本不倾斜，区分4个角点
	std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
	std::vector<float> vY = { left_top_point.y ,left_bottom_point.y ,right_bottom_point.y ,right_top_point.y };
	std::vector<int> index_by_Y = CommonFun::sort_index(vY, 1);//获取较大的Y值索引  0 1 是Y较大的下面的点  2 3是Y较小的上面的点 
	if (vPoint[index_by_Y[0]].x < vPoint[index_by_Y[1]].x) {
		left_bottom_point = vPoint[index_by_Y[0]];
		right_bottom_point = vPoint[index_by_Y[1]];
	} else {
		left_bottom_point = vPoint[index_by_Y[1]];
		right_bottom_point = vPoint[index_by_Y[0]];
	}
	if (vPoint[index_by_Y[2]].x < vPoint[index_by_Y[3]].x) {
		left_top_point = vPoint[index_by_Y[2]];
		right_top_point = vPoint[index_by_Y[3]];
	} else {
		left_top_point = vPoint[index_by_Y[3]];
		right_top_point = vPoint[index_by_Y[2]];
	}

	if (1) {
		//绘制外接矩形
		std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
		//3D点云数据转化
		std::vector<cv::Point3f> CloudPoints;//空间点
		for (const auto& point : vPoint) {
			{
				CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			}
		}

		//把点投影到2D平面上
		Eigen::Matrix4d CamPose = EigenFunc::TransEulerPostion2Matrix({ 0, 0, 0, 0, 0, 0 });
		cv::Mat CamPoseMat = EigenFunc::MatrixToMat4d(CamPose);
		cv::Mat Rvec, Tvec;
		EigenFunc::HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

		std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
		cv::projectPoints(CloudPoints, Rvec, Tvec, m_intriMatrix, m_distorMatrix, ImagePoints);

		std::vector<cv::Point2f> ImagePoints2{ ImagePoints[1],ImagePoints[2] ,ImagePoints[3],ImagePoints[0] };
		//绘制外接矩形
		EigenFunc::lines(image, ImagePoints, ImagePoints2, sc, 3);
	}
}

void VisualCommon::DrowPlate(cv::Mat& image, double height)
{
	cv::Scalar color;
	color[0] = 255; color[1] = 0; color[2] = 0;
	PointCloud::Ptr palltWordCloud(new PointCloud);//此为世界坐标系
	PointT point_temp1;
	point_temp1.x = m_plateMin.x / 1000;
	point_temp1.y = m_plateMin.y / 1000;
	point_temp1.z = height / 1000;
	palltWordCloud->points.push_back(point_temp1);

	PointT point_temp2;
	point_temp2.x = m_plateMin.x / 1000;
	point_temp2.y = m_plateMax.y / 1000;
	point_temp2.z = height / 1000;
	palltWordCloud->points.push_back(point_temp2);

	PointT point_temp3;
	point_temp3.x = m_plateMax.x / 1000;
	point_temp3.y = m_plateMin.y / 1000;
	point_temp3.z = height / 1000;
	palltWordCloud->points.push_back(point_temp3);

	PointT point_temp4;
	point_temp4.x = m_plateMax.x / 1000;
	point_temp4.y = m_plateMax.y / 1000;
	point_temp4.z = height / 1000;
	palltWordCloud->points.push_back(point_temp4);
	DrawPointCloud(palltWordCloud, color, image);

}
void VisualCommon::DrowBox(cv::Mat& image, cv::RotatedRect rect)
{
	//通过Rect找到四个角点坐标
	cv::Point2f box[4];
	rect.points(box);

	cv::Point p1(box[0].x, box[0].y);
	cv::Point p2(box[1].x, box[1].y);
	cv::Point p3(box[2].x, box[2].y);
	cv::Point p4(box[3].x, box[3].y);
	std::vector<cv::Point> pts = { p1, p2, p3, p4 };

	cv::polylines(image, pts, true, cv::Scalar(0, 0, 255), 2, cv::LINE_AA, 0);
}

void VisualCommon::DrawRcog(cv::Mat& image, std::vector<cv::Point>& point)
{
	cv::polylines(image, point, true, cv::Scalar(0, 255, 0), 2, cv::LINE_AA, 0);
}

QByteArray VisualCommon::ImgEncodeToByte(const cv::Mat& Image, const std::string& ext)
{
	if (Image.empty()) {
		return	{};
	}
	//图片编码
	std::vector<uchar> vuBuf;
	cv::imencode(ext, Image, vuBuf);
	QByteArray qbaImage(reinterpret_cast<const char*>(vuBuf.data()), vuBuf.size());//https://www.jb51.cc/c/116955.html
	QByteArray qbaImage1 = qbaImage.toBase64();

	QTextCodec* tc = QTextCodec::codecForName("UTF8");//https://blog.csdn.net/liukang325/article/details/80986472?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control
	QString qbaImage2 = tc->toUnicode(qbaImage1);

	return qbaImage2.toUtf8();
}

void VisualCommon::SavePCDFile(const PointCloud::Ptr& cloud, const QString& name)
{
	cloud->height = 1;
	cloud->width = cloud->points.size();
	if (cloud->width < 1) {
		return;
	}
	PclFun::ShowCloud(cloud, name.toStdString());
	pcl::io::savePCDFile(name.toStdString() + ".pcd", *cloud);
}

bool VisualCommon::SortSkuPos(QVector<visual::BoxInfo>& pos)
{
	using visual::BoxInfo;
	// x由小变大，y由小变大
	auto fun1 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = p2.pos.x + p2.pos.y;
		int num1 = p1.pos.x + p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y < p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由小变大
	auto fun2 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = -p2.pos.x + p2.pos.y;
		int num1 = -p1.pos.x + p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y < p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	auto fun3 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = p2.pos.x - p2.pos.y;
		int num1 = p1.pos.x - p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y > p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	auto fun4 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = -p2.pos.x - p2.pos.y;
		int num1 = -p1.pos.x - p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y > p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};

	switch (m_dir) {
	case 3:
		std::sort(pos.begin(), pos.end(), fun4);
		break;
	case 4:
		std::sort(pos.begin(), pos.end(), fun3);
		break;
	case 1:
		std::sort(pos.begin(), pos.end(), fun1);
		break;
	case 2:
		std::sort(pos.begin(), pos.end(), fun2);
		break;
	default:
		return false;
		break;
	}

	return true;
}
